#pragma once

#include "common.h"
#include <complement.h>



struct MarkerPixel
{
	Point3f m_3d;
	uchar i;
	float gx, gy;
};

struct FramePixel
{
	float i;
	float gx, gy;
};

typedef Mat_<Vec3f> superFrame;

class Camera
{
public:
	Camera(const Mat_<double> &_R, const Mat_<double> &_t) :R(_R), t(_t) {};
	~Camera() {};
	Mat_<double> R;
	Mat_<double> t;
	template<typename T>
	void TransformToCameraPlane(const Point3_<T> &s, Point3_<T>&d);
	template<typename T>
	void TransformToCameraPlane(const vector<Point3_<T>> &src, vector<Point3_<T>> &dst);
};

class IntrinsicMatrix
{
public:
	template<typename T2>
	IntrinsicMatrix(const Mat_<T2>&_K) :K(_K.clone()), fx(K.at<T2>(0)), fy(K.at<T2>(4)), cx(K.at<T2>(2)), cy(K.at<T2>(5)) {};
	~IntrinsicMatrix() {};
	Mat_<double> K;
	double fx, fy, cx, cy;
	template<typename T, typename T2>
	void projectToImage(const vector<Point3_<T>> &s, vector<Point_<T2>> &dst);
	template<typename T, typename T2>
	void projectToImage(const Point3_<T> &s, Point_<T2> &d);
private:

};



//check ok!
template<typename T,typename T2>
void IntrinsicMatrix::projectToImage(const vector<Point3_<T>> &s, vector<Point_<T2>> &d)
{
	d.resize(s.size());
	for (size_t i = 0; i < s.size(); i++)
	{
		projectToImage(s[i], d[i]);
	}
}

template<typename T,typename T2>
inline void IntrinsicMatrix::projectToImage(const Point3_<T> &s, Point_<T2> &d)
{
	const auto & k = (const double*)K.data;
	d.x = (s.x*k[0] / s.z)+k[2];
	d.y = (s.y*k[4]/ s.z)+k[5];
}



template<typename T>
void Camera::TransformToCameraPlane(const Point3_<T> &s, Point3_<T>&d)
{
	const auto &r = (const double*)R.data;
	const auto &tt = (const double*)t.data;
	d.x = s.x*r[0] + s.y*r[1] + s.z*r[2] + tt[0];
	d.y = s.x*r[3] + s.y*r[4] + s.z*r[5] + tt[1];
	d.z = s.x*r[6] + s.y*r[7] + s.z*r[8] + tt[2];
}


//check ok!
template<typename T>
void Camera::TransformToCameraPlane(const vector<Point3_<T>> &src, vector<Point3_<T>> &dst)
{
	dst.clear();
	dst.resize(src.size());

	for (size_t i = 0; i < src.size(); i++)
	{
		const auto &s = src[i];
		auto &d = dst[i];
		TransformToCameraPlane(s, d);
	}
}



double computeEnergyLM(vector<MarkerPixel>& M, const superFrame & I, IntrinsicMatrix & K, Camera & C, const float * p, const vector<Point3f>& kp3, const vector<Point2f>& kp2, double w1, double w2);

void computeGradient(const Mat & _src, Mat & output);

void computeGradient(const Mat & _src, Mat & gx, Mat & gy);

void computeJacobian(vector<MarkerPixel>& M, const Mat & I, IntrinsicMatrix & K, Camera & C, const float * p, const vector<Point3f>& kp3, const vector<Point2f>& kp2, double w1, double w2, Mat & J);
